/* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8:
 * $Id: query-state.cpp,v 1.1 2004/09/12 03:40:40 poine Exp $
 *
 * (c) 2003 Trammell Hudson <hudson@rotomotion.com>
 *
 * Simple command line program to retrieve the state from the
 * flight controller.
 *
 *************
 *
 *  This file is part of the autopilot simulation package.
 *
 *  Autopilot is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Autopilot is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Autopilot; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cmath>
#include <unistd.h>

#include "macros.h"
#include "timer.h"
#include "byteorder.h"
#include <getoptions/getoptions.h>
#include <state/Server.h>
#include <mat/Conversions.h>
#include <mat/Nav.h>

using namespace libstate;
using namespace libmat;
using namespace util;
using namespace std;


static int		verbose;

static int
help( void )
{
	cerr <<
"Usage: query-state [options]\n"
"\n"
"	-h | --help			This help\n"
"	-v | --verbose			Increase verbosity\n"
"	-V | --version			Version\n"
"	-s | --server hostname		State server\n"
"	-p | --port port		State port\n"
"\n"
	<< endl;

	return -10;
}

static int
version( void )
{
	cerr << "$Id: query-state.cpp,v 1.1 2004/09/12 03:40:40 poine Exp $" << endl;
	return -10;
}


int
main(
	int			argc,
	char **			argv
)
{
	const char *		program		= argv[0];
	const char *		server_host	= "127.0.0.1";
	int			server_port	= 2002;


	int rc = getoptions( &argc, &argv,
		"h|?|help&",		help,
		"V|version&",		version,
		"v|verbose+",		&verbose,
		"s|server=s",		&server_host,
		"p|port=i",		&server_port,
		0
	);

	if( rc == -10 )
		return EXIT_FAILURE;
	if( rc < 0 )
		return help();

	state_t			state;
	Server			server( server_host, server_port );

	server.handle( AHRS_STATE, Server::process_ahrs, (void*) &state );

	while( 1 )
	{

		int type = server.get_packet();

		if( type < 0 )
		{
			cout << "Read error from server" << endl;
			return -1;
		}

		if( type != AHRS_STATE )
		{
			if( verbose > 1 )
				cout << "Got packet of type " << type << endl;
			continue;
		}

		break;
	}

	printf( "Attitude: % 8.4f % 8.4f % 8.4f (deg)\n",
		state.phi * C_RAD2DEG,
		state.theta * C_RAD2DEG,
		state.psi * C_RAD2DEG
	);

	printf( "Rates:    % 8.4f % 8.4f % 8.4f (deg/s)\n",
		state.p * C_RAD2DEG,
		state.q * C_RAD2DEG,
		state.r * C_RAD2DEG
	);

	printf( "Accel:    % 8.4f % 8.4f % 8.4f (m/s/s)\n",
		state.ax,
		state.ay,
		state.az
	);

	printf( "Mag:      % 8.4f % 8.4f % 8.4f (units)\n",
		state.mx,
		state.my,
		state.mz
	);

	return 0;
}
